#include "optimizer.h"

Optimizer::Optimizer() {

}

void Optimizer::OptimizeGN(State& state, const Eigen::Vector2d &velocity, const Eigen::Vector3d &pose) {
    int iterations = 100;
    double cost = 0, last_cost = 0;

    Eigen::Matrix<double, 5, 5> pose_covariance = Eigen::Matrix<double, 5, 5>::Identity();
    pose_covariance(0,0) = std::pow(0.01,2);
    pose_covariance(1,1) = std::pow(0.01,2);
    pose_covariance(2,2) = std::pow(0.01,2);
    pose_covariance(3,3) = std::pow(0.01,2);
    pose_covariance(4,4) = std::pow(0.01,2);

    Eigen::Matrix<double, 5, 5> information_matrix = pose_covariance.inverse();

    // Eigen::Matrix<double, 2, 2> pose_covariance = Eigen::Matrix<double, 2, 2>::Identity();
    // pose_covariance(0,0) = std::pow(0.00017,2);
    // pose_covariance(1,1) = std::pow(0.00017,2);

    // Eigen::Matrix<double, 2, 2> information_matrix = pose_covariance.inverse();   
    
    // Eigen::Matrix<double, 8, 8> information_matrix = state.cov.inverse();

    for (int iter = 0; iter < iterations; iter++) {
        cost = 0;
        Eigen::Matrix<double, 5, 1> residual; 
        Eigen::Matrix<double, 5, 8> jacobian;

        // Eigen::Matrix<double, 2, 1> residual; 
        // Eigen::Matrix<double, 2, 8> jacobian;
        PoseObserve::ComputeResidualAndJacobian(state, pose, velocity, residual, jacobian);
        cost += Chi2(residual, pose_covariance.inverse());

        cout << "cost == : " << cost << endl;
        cout << "residual == : " << residual.transpose() << endl;
        cout << "jacobian == : \n" << jacobian << endl;

        Eigen::Matrix<double, 8, 8> H = Eigen::Matrix<double, 8, 8>::Zero();
        Eigen::Matrix<double, 8, 1> b = Eigen::Matrix<double, 8, 1>::Zero();

        H += jacobian.transpose() * information_matrix * jacobian;
        b += -jacobian.transpose() * information_matrix * residual;

  		cout << "H == : \n" << H << endl;
        // cout << "H.inverse() == : \n" << H.inverse() << endl;

        cout << "b == : \n" << b << endl;
  		// cout << "residual ================== : " << residual.transpose() << endl;

        // solve dx
        Eigen::Matrix<double, 8, 1> dx = H.ldlt().solve(b);
        // Eigen::Matrix<double, 8, 1> dx = H.inverse() * (b);
        if (isnan(dx[0])) {
            cout << "result is nan!" << endl;
            break;
        }

        if (iter > 0 && cost >= last_cost) {
            // 误差增长了，说明近似的不够好
            cout << "cost: " << cost << ", last cost: " << last_cost << endl;
            break;
        }

        update_state(state, dx);

        last_cost = cost;

        cout << "iteration == : " << iter << " cost == : " << cout.precision(12) << cost << endl;
        cout << "dx ================== : " << dx.transpose() << endl;
    }
}

void Optimizer::OptimizeGN(State& last_state, State& state, const Eigen::Vector3d &last_velocity, const Eigen::Vector3d &velocity, const Eigen::Vector3d &delta_odometry, const Eigen::Vector3d &last_pose, const Eigen::Vector3d &curr_pose) {
    int iterations = 100;
    double cost = 0, last_cost = 0;

    Eigen::Matrix<double, 5, 5> last_pose_covariance = Eigen::Matrix<double, 5, 5>::Identity();
    last_pose_covariance(0,0) = std::pow(0.01,2);
    last_pose_covariance(1,1) = std::pow(0.01,2);
    last_pose_covariance(2,2) = std::pow(0.01,2);
    last_pose_covariance(3,3) = std::pow(0.01,2);
    last_pose_covariance(4,4) = std::pow(0.01,2);
    Eigen::Matrix<double, 5, 5> last_information_matrix = last_pose_covariance.inverse();

    Eigen::Matrix<double, 5, 5> curr_pose_covariance = Eigen::Matrix<double, 5, 5>::Identity();
    curr_pose_covariance(0,0) = std::pow(0.01,2);
    curr_pose_covariance(1,1) = std::pow(0.01,2);
    curr_pose_covariance(2,2) = std::pow(0.01,2);
    curr_pose_covariance(3,3) = std::pow(0.01,2);
    curr_pose_covariance(4,4) = std::pow(0.01,2);
    Eigen::Matrix<double, 5, 5> curr_information_matrix = curr_pose_covariance.inverse();
    
    cout << "curr_information_matrix == : \n" << curr_information_matrix << endl;

    Eigen::Matrix<double, 10, 10> information_matrix = Eigen::Matrix<double, 10, 10>::Identity();
    information_matrix.block<5,5>(0,0) = last_information_matrix;
    information_matrix.block<5,5>(5,5) = curr_information_matrix;

    State last_state_tmp = last_state; 
    State state_tmp = state;

    for (int iter = 0; iter < iterations; iter++) {

        Eigen::Matrix<double, 5, 1> residualXi; 
        Eigen::Matrix<double, 5, 8> jacobianXi;

        Eigen::Matrix<double, 5, 1> residualXj; 
        Eigen::Matrix<double, 5, 5> jacobianXj;

        Eigen::Matrix<double, 5, 5> jacobianXij;
        Eigen::Matrix<double, 5, 8> jacobianXji;
        PoseObserve::ComputeResidualAndJacobianPoseGraph(last_state, last_pose, last_velocity.head<2>(), state, curr_pose, velocity.head<2>(), residualXi, jacobianXi, residualXj, jacobianXj, jacobianXij, jacobianXji);
 #if 1
        // PoseObserve::ComputeResidualAndNumericalJacobian(last_state, last_pose, last_velocity.head<2>(), state, curr_pose, velocity.head<2>(), residualXi, jacobianXi, residualXj, jacobianXj, jacobianXij, jacobianXji);
        // CeresOptimizer::ComputeResidualAndNumericalJacobian(last_state, last_pose, last_velocity.head<2>(), state, curr_pose, velocity.head<2>(), residualXi, jacobianXi, residualXj, jacobianXj, jacobianXij, jacobianXji);
        CeresOptimizer::ComputeResidualAndAutoJacobian(last_state, last_pose, last_velocity.head<2>(), state, curr_pose, velocity.head<2>(), residualXi, jacobianXi, residualXj, jacobianXj, jacobianXij, jacobianXji);
#endif
        cost += Chi2(residualXi, last_information_matrix);
        cost += Chi2(residualXj, curr_information_matrix);

        Eigen::Matrix<double, 10, 1> residual;
        residual.block<5,1>(0,0) = residualXi;
        residual.block<5,1>(5,0) = residualXj;

        Eigen::Matrix<double,10,13> jacobian = Eigen::Matrix<double,10,13>::Zero();
        jacobian.block<5,8>(0,0) = jacobianXi;
        jacobian.block<5,5>(0,8) = jacobianXij;
        jacobian.block<5,8>(5,0) = jacobianXji;
        jacobian.block<5,5>(5,8) = jacobianXj;

        // cout << "residual == : " << residual.transpose() << endl;
        cout << "jacobian == : \n" << jacobian << endl;

        Eigen::Matrix<double, 13, 13> H = Eigen::Matrix<double, 13, 13>::Zero();
        Eigen::Matrix<double, 13, 1> b = Eigen::Matrix<double, 13, 1>::Zero();

        H += jacobian.transpose() * information_matrix * jacobian;
        b += -jacobian.transpose() * information_matrix * residual;

        // cout << "H == : \n" << H << endl;
        // cout << "H.inverse() == : \n" << H.inverse() << endl;

        // cout << "b == : \n" << b << endl;
        // cout << "residual ================== : " << residual.transpose() << endl;
        // cout << "cost============= : " << cost  << endl;

        // solve dx
        Eigen::Matrix<double, 13, 1> dx = H.ldlt().solve(b);
        // Eigen::Matrix<double, 8, 1> dx = H.inverse() * (b);
        if (isnan(dx[0])) {
            cout << "result is nan!" << endl;

            last_state = last_state_tmp;
            state = state_tmp;
            break;
        }

        if (iter > 0 && cost >= last_cost) {
            last_state = last_state_tmp;
            state = state_tmp;
            // 误差增长了，说明近似的不够好
            cout << "cost: " << cost << ", last cost: " << last_cost << endl;
            break;
        }

        update_state(last_state, state, dx);

        last_cost = cost;

        cout << "iteration == : " << iter << " cost == : " << cost << endl;
        cout << "dx ================== : " << dx.transpose() << endl;
    }
}

void Optimizer::OptimizeLM(State& last_state, State& state, const Eigen::Vector3d &last_velocity, const Eigen::Vector3d &velocity, const Eigen::Vector3d &delta_odometry, const Eigen::Vector3d &last_pose, const Eigen::Vector3d &curr_pose) {
    
    float cost_old = 0.0f;
    bool recompute = true;
    double eps = 10000;
    int runs = 0;
    double lamda =  1e-3;
    double lam_mult = 10;
    double min_dcost = 1e-6;

    Eigen::Matrix<double, 13, 13> Hess = Eigen::Matrix<double, 13, 13>::Zero();
    Eigen::Matrix<double, 13, 1> grad = Eigen::Matrix<double, 13, 1>::Zero();

    Eigen::Matrix<double, 10, 1> residual = Eigen::Matrix<double, 10, 1>::Zero();

    Eigen::Matrix<double, 5, 5> last_pose_covariance = Eigen::Matrix<double, 5, 5>::Identity();
    last_pose_covariance(0,0) = std::pow(0.01,2);
    last_pose_covariance(1,1) = std::pow(0.01,2);
    last_pose_covariance(2,2) = std::pow(0.01,2);
    last_pose_covariance(3,3) = std::pow(0.01,2);
    last_pose_covariance(4,4) = std::pow(0.01,2);
    Eigen::Matrix<double, 5, 5> last_information_matrix = last_pose_covariance.inverse();

    Eigen::Matrix<double, 5, 5> curr_pose_covariance = Eigen::Matrix<double, 5, 5>::Identity();
    curr_pose_covariance(0,0) = std::pow(0.01,2);
    curr_pose_covariance(1,1) = std::pow(0.01,2);
    curr_pose_covariance(2,2) = std::pow(0.01,2);
    curr_pose_covariance(3,3) = std::pow(0.01,2);
    curr_pose_covariance(4,4) = std::pow(0.01,2);
    Eigen::Matrix<double, 5, 5> curr_information_matrix = curr_pose_covariance.inverse();
    
    // cout << "curr_information_matrix == : \n" << curr_information_matrix << endl;

    Eigen::Matrix<double, 10, 10> information_matrix = Eigen::Matrix<double, 10, 10>::Identity();
    information_matrix.block<5,5>(0,0) = last_information_matrix;
    information_matrix.block<5,5>(5,5) = curr_information_matrix;
    Eigen::Matrix<double, 5, 1> residualXi; 
    Eigen::Matrix<double, 5, 8> jacobianXi;

    Eigen::Matrix<double, 5, 1> residualXj; 
    Eigen::Matrix<double, 5, 5> jacobianXj;

    Eigen::Matrix<double, 5, 5> jacobianXij;
    Eigen::Matrix<double, 5, 8> jacobianXji;
    PoseObserve::ComputeResidualAndJacobianPoseGraph(last_state, last_pose, last_velocity.head<2>(), state, curr_pose, velocity.head<2>(), residualXi, jacobianXi, residualXj, jacobianXj, jacobianXij, jacobianXji);
    cost_old += Chi2(residualXi, last_information_matrix);
    cost_old += Chi2(residualXj, curr_information_matrix);

    // cout << "cost_old =========== : " << cost_old << endl;

    while (runs < 100 && lamda < 1e10 && eps > 1e-6) {
        if (recompute) {
            Hess.setZero();
            grad.setZero();
            PoseObserve::ComputeResidualAndJacobianPoseGraph(last_state, last_pose, last_velocity.head<2>(), state, curr_pose, velocity.head<2>(), residualXi, jacobianXi, residualXj, jacobianXj, jacobianXij, jacobianXji);
            Eigen::Matrix<double,10,13> H = Eigen::Matrix<double,10,13>::Zero();
            H.block<5,8>(0,0) = jacobianXi;
            H.block<5,5>(0,8) = jacobianXij;
            H.block<5,8>(5,0) = jacobianXji;
            H.block<5,5>(5,8) = jacobianXj;
            residual.block<5,1>(0,0) = residualXi;
            residual.block<5,1>(5,0) = residualXj;
            grad.noalias() += -H.transpose() * information_matrix * residual;
            Hess.noalias() += H.transpose() * information_matrix *  H;
        }

        // Solve Levenberg iteration
        Eigen::Matrix<double,13, 13> Hess_l = Hess;
        for (size_t r=0; r < (size_t)Hess.rows(); r++) {
            Hess_l(r,r) *= (1.0 + lamda);
            //std::cout << " lamda : " << lamda << std::endl;
        }

        Eigen::Matrix<double,13,1> dx = Hess_l.colPivHouseholderQr().solve(grad);

        // std::cout << " iterator idx and dx = : " << runs << ", " << dx.transpose() << std::endl;

        State last_state_tmp = last_state;
        State state_tmp = state;
        update_state(last_state_tmp, state_tmp, dx);
        PoseObserve::ComputeResidualAndJacobianPoseGraph(last_state_tmp, last_pose, last_velocity.head<2>(), state_tmp, curr_pose, velocity.head<2>(), residualXi, jacobianXi, residualXj, jacobianXj, jacobianXij, jacobianXji);
        
        double cost = 0.0;
        cost += Chi2(residualXi, last_information_matrix);
        cost += Chi2(residualXj, curr_information_matrix);
    
        // Check if converged
        if (cost <= cost_old && (cost_old - cost) / cost_old < min_dcost) {
            eps = 0;

            update_state(last_state, state, dx);
 
            // std::cout << "converged" << std::endl;
            // std::cout << " cost : " << cost << std::endl;
            // std::cout << " cost_old : " << cost_old << std::endl;
            // std::cout << " runs : " << runs << std::endl;
            // std::cout << " dx ===== : " << dx.transpose() << std::endl;
            break;
        }

        // If cost is lowered, accept step
        if (cost <= cost_old) {
            // std::cout << " cost_old : " << cost_old << std::endl;
            // std::cout << " cost : " << cost << std::endl;                
            // std::cout << " runs : " << runs << std::endl;

            recompute = true;
            cost_old = cost;

            update_state(last_state, state, dx);

            runs++;
            lamda = lamda / lam_mult;
            eps = dx.norm();

        } else {
            recompute = false;
            lamda = lamda * lam_mult;
            continue;
        }      
    }
}

void Optimizer::OptimizeLMCh6(State& last_state, State& state, const Eigen::Vector3d &last_velocity, const Eigen::Vector3d &velocity, const Eigen::Vector3d &delta_odometry, const Eigen::Vector3d &last_pose, const Eigen::Vector3d &curr_pose, const Eigen::Vector3d& last_imu, const Eigen::Vector3d& curr_imu, const double& dt) {
    float cost_old = 0.0f;
    bool recompute = true;
    double eps = 10000;
    int runs = 0;
    double lamda =  1e-3;
    double lam_mult = 10;
    double min_dcost = 1e-6;

    State last_imu_state = last_state; 
    State imu_state = state;

    Eigen::Matrix<double, 13, 13> Hess = Eigen::Matrix<double, 13, 13>::Zero();
    Eigen::Matrix<double, 13, 1> grad = Eigen::Matrix<double, 13, 1>::Zero();

    Eigen::Matrix<double, 5, 1> imufacotr_residual = Eigen::Matrix<double, 5, 1>::Zero();

    // there is exist bug
    Eigen::Matrix<double, 8, 8> imufacotr_delta_cov = state.cov-last_state.cov;

    // temporary set
    imufacotr_delta_cov = 1e-1 * Eigen::Matrix<double, 8, 8>::Ones();
    imufacotr_delta_cov(0,0) = std::pow(0.1,2);
    imufacotr_delta_cov(1,1) = std::pow(0.1,2);
    imufacotr_delta_cov(2,2) = std::pow(0.1,2);
    imufacotr_delta_cov(3,3) = std::pow(0.1,2);
    imufacotr_delta_cov(4,4) = std::pow(0.1,2);
    imufacotr_delta_cov(5,5) = std::pow(0.1,2);
    imufacotr_delta_cov(6,6) = std::pow(0.1,2);
    imufacotr_delta_cov(7,7) = std::pow(0.1,2);
    // cout << "imufacotr_delta_cov =========== : " << imufacotr_delta_cov << endl;

    Eigen::Matrix<double, 5, 5> imufacotr_information_matrix = Eigen::Matrix<double, 5, 5>::Identity();
    imufacotr_information_matrix.block<5,5>(0,0) = (imufacotr_delta_cov.block<5,5>(0,0)).inverse();

    Eigen::Matrix<double, 5, 8> imufacotr_jacobianXi = Eigen::Matrix<double, 5, 8>::Zero();
    Eigen::Matrix<double, 5, 5> imufacotr_jacobianXj = Eigen::Matrix<double, 5, 5>::Zero();

    ImuFactor::ComputeResidualAndJacobianPoseGraph(last_imu_state, imu_state, last_state, state, imufacotr_residual, imufacotr_jacobianXi, imufacotr_jacobianXj, last_imu, curr_imu, dt);
    cost_old += Chi2(imufacotr_residual, imufacotr_information_matrix);


    Eigen::Matrix<double, 5, 5> last_pose_covariance = Eigen::Matrix<double, 5, 5>::Identity();
    last_pose_covariance(0,0) = std::pow(0.01,2);
    last_pose_covariance(1,1) = std::pow(0.01,2);
    last_pose_covariance(2,2) = std::pow(0.01,2);
    last_pose_covariance(3,3) = std::pow(0.01,2);
    last_pose_covariance(4,4) = std::pow(0.01,2);
    Eigen::Matrix<double, 5, 5> last_information_matrix = last_pose_covariance.inverse();

    Eigen::Matrix<double, 5, 5> curr_pose_covariance = Eigen::Matrix<double, 5, 5>::Identity();
    curr_pose_covariance(0,0) = std::pow(0.01,2);
    curr_pose_covariance(1,1) = std::pow(0.01,2);
    curr_pose_covariance(2,2) = std::pow(0.01,2);
    curr_pose_covariance(3,3) = std::pow(0.01,2);
    curr_pose_covariance(4,4) = std::pow(0.01,2);
    Eigen::Matrix<double, 5, 5> curr_information_matrix = curr_pose_covariance.inverse();
    
    // cout << "curr_information_matrix == : \n" << curr_information_matrix << endl;

    Eigen::Matrix<double, 10, 10> pose_information_matrix = Eigen::Matrix<double, 10, 10>::Identity();
    pose_information_matrix.block<5,5>(0,0) = last_information_matrix;
    pose_information_matrix.block<5,5>(5,5) = curr_information_matrix;
    Eigen::Matrix<double, 5, 1> residualXi; 
    Eigen::Matrix<double, 5, 8> jacobianXi;

    Eigen::Matrix<double, 5, 1> residualXj; 
    Eigen::Matrix<double, 5, 5> jacobianXj;

    Eigen::Matrix<double, 5, 5> jacobianXij;
    Eigen::Matrix<double, 5, 8> jacobianXji;
    PoseObserve::ComputeResidualAndJacobianPoseGraph(last_state, last_pose, last_velocity.head<2>(), state, curr_pose, velocity.head<2>(), residualXi, jacobianXi, residualXj, jacobianXj, jacobianXij, jacobianXji);
    cost_old += Chi2(residualXi, last_information_matrix);
    cost_old += Chi2(residualXj, curr_information_matrix);

    // // cout << "cost_old =========== : " << cost_old << endl;

    while (runs < 100 && lamda < 1e10 && eps > 1e-6) {
        if (recompute) {
            Hess.setZero();
            grad.setZero();
            PoseObserve::ComputeResidualAndJacobianPoseGraph(last_state, last_pose, last_velocity.head<2>(), state, curr_pose, velocity.head<2>(), residualXi, jacobianXi, residualXj, jacobianXj, jacobianXij, jacobianXji);
            
            Eigen::Matrix<double,5,13> H = Eigen::Matrix<double,5,13>::Zero();
            H.block<5,8>(0,0) = jacobianXi;
            H.block<5,5>(0,8) = jacobianXij;
            grad.noalias() += -H.transpose() * last_information_matrix * residualXi;
            Hess.noalias() += H.transpose() * last_information_matrix *  H;

            H = Eigen::Matrix<double,5,13>::Zero();
            H.block<5,8>(0,0) = jacobianXji;
            H.block<5,5>(0,8) = jacobianXj;
            grad.noalias() += -H.transpose() * curr_information_matrix * residualXj;
            Hess.noalias() += H.transpose() * curr_information_matrix *  H;

            H = Eigen::Matrix<double,5,13>::Zero();
            H.block<5,8>(0,0) = imufacotr_jacobianXi;
            H.block<5,5>(0,8) = imufacotr_jacobianXj;
            grad.noalias() += -H.transpose() * imufacotr_information_matrix * imufacotr_residual;
            Hess.noalias() += H.transpose() * imufacotr_information_matrix *  H;            
        }

        // Solve Levenberg iteration
        Eigen::Matrix<double,13, 13> Hess_l = Hess;
        for (size_t r=0; r < (size_t)Hess.rows(); r++) {
            Hess_l(r,r) *= (1.0 + lamda);
            //std::cout << " lamda : " << lamda << std::endl;
        }

        Eigen::Matrix<double,13,1> dx = Hess_l.colPivHouseholderQr().solve(grad);

        // std::cout << " iterator idx and dx = : " << runs << ", " << dx.transpose() << std::endl;

        State last_state_tmp = last_state;
        State state_tmp = state;
        update_state(last_state_tmp, state_tmp, dx);
        PoseObserve::ComputeResidualAndJacobianPoseGraph(last_state_tmp, last_pose, last_velocity.head<2>(), state_tmp, curr_pose, velocity.head<2>(), residualXi, jacobianXi, residualXj, jacobianXj, jacobianXij, jacobianXji);
        ImuFactor::ComputeResidualAndJacobianPoseGraph(last_imu_state, imu_state, last_state_tmp, state_tmp, imufacotr_residual, imufacotr_jacobianXi, imufacotr_jacobianXj, last_imu, curr_imu, dt);

        double cost = 0.0;
        cost += Chi2(residualXi, last_information_matrix);
        cost += Chi2(residualXj, curr_information_matrix);
        cost += Chi2(imufacotr_residual, imufacotr_information_matrix);

        // std::cout << " cost : " << cost << std::endl;

        // Check if converged
        if (cost <= cost_old && (cost_old - cost) / cost_old < min_dcost) {
            eps = 0;

            update_state(last_state, state, dx);
            update_current_obs_state(last_imu_state, imu_state, last_state, last_imu, curr_imu, dt);

            // std::cout << "converged" << std::endl;
            // std::cout << " cost : " << cost << std::endl;
            // std::cout << " cost_old : " << cost_old << std::endl;
            // std::cout << " runs : " << runs << std::endl;
            // std::cout << " dx ===== : " << dx.transpose() << std::endl;
            break;
        }

        // If cost is lowered, accept step
        if (cost <= cost_old) {
            // std::cout << " cost_old : " << cost_old << std::endl;
            // std::cout << " cost : " << cost << std::endl;                
            // std::cout << " runs : " << runs << std::endl;

            recompute = true;
            cost_old = cost;

            update_state(last_state, state, dx);
            update_current_obs_state(last_imu_state, imu_state, last_state, last_imu, curr_imu, dt);

            runs++;
            lamda = lamda / lam_mult;
            eps = dx.norm();

        } else {
            recompute = false;
            lamda = lamda * lam_mult;
            continue;
        }      
    }
}

double Optimizer::Chi2(const Eigen::Matrix<double, 5, 1>& residual, const Eigen::Matrix<double, 5, 5>& information_matrix) {
	return residual.transpose() * information_matrix * residual;
}

double Optimizer::Chi2(const Eigen::Matrix<double, 2, 1>& residual, const Eigen::Matrix<double, 2, 2>& information_matrix) {
    return residual.transpose() * information_matrix * residual;
}

double Optimizer::Chi2(const Eigen::Matrix<double, 3, 1>& residual, const Eigen::Matrix<double, 3, 3>& information_matrix) {
    return residual.transpose() * information_matrix * residual;
}

void Optimizer::update_state(State& state, const Eigen::Matrix<double, 8, 1>& dx) {
	state.p += dx.block<2,1>(0,0);
	state.v += dx.block<2,1>(2,0);
	state.theta += dx[4];
	state.theta = math_utils::normalize_angle(state.theta);
    // state.theta = math_utils::theta_normalize(state.theta);
	state.ba += dx.block<2,1>(5,0); 
	state.bg += dx(7,0);

    std::cout << "current state.theta ============= : " << state.theta << std::endl;
    std::cout << "current state.p ============= : " << state.p.transpose() << std::endl;
    std::cout << "current state.v ============= : " << state.v.transpose() << std::endl;
    std::cout << "current state.ba ============= : " << state.ba.transpose() << std::endl;
    std::cout << "current state.bg ============= : " << state.bg << std::endl;
}

void Optimizer::update_state(State& last_state, State& state, const Eigen::Matrix<double, 13, 1>& dx) {
    last_state.p += dx.block<2,1>(0,0);
    last_state.v += dx.block<2,1>(2,0);
    last_state.theta += dx[4];
    // last_state.theta = math_utils::normalize_angle(last_state.theta);
    last_state.theta = math_utils::theta_normalize(last_state.theta);
    last_state.ba += dx.block<2,1>(5,0); 
    last_state.bg += dx(7,0);

    state.p += dx.block<2,1>(8,0);
    state.v += dx.block<2,1>(10,0);
    state.theta += dx[12];
    // state.theta = math_utils::normalize_angle(state.theta);
    state.theta = math_utils::theta_normalize(state.theta);
    state.ba = last_state.ba; 
    state.bg = last_state.bg;
}

void Optimizer::update_current_obs_state(State& imu_state, State& last_imu_state, const State& last_state, const Eigen::Vector3d& last_imu, const Eigen::Vector3d& curr_imu, const double& dt) {
    last_imu_state.ba = last_state.ba;
    last_imu_state.bg = last_state.bg;
    imu_state.ba = last_imu_state.ba;
    imu_state.bg = last_imu_state.bg;

    Eigen::Vector3d last_imu_unbias(last_imu[0], last_imu[1], last_imu[2]);
    Eigen::Vector3d curr_imu_unbias(curr_imu[0], curr_imu[1], curr_imu[2]);
    last_imu_unbias[0] -= last_imu_state.ba[0];
    curr_imu_unbias[0] -= last_imu_state.ba[0];
    last_imu_unbias[1] -= last_imu_state.ba[1];
    curr_imu_unbias[1] -= last_imu_state.ba[1];
    last_imu_unbias[2] -= last_imu_state.bg;
    curr_imu_unbias[2] -= last_imu_state.bg;

    // std::cout << "dt ============= : " << dt << ", " << init_bias_ << std::endl;
    Eigen::Vector3d imu_unbias_hat = 0.5 * (last_imu_unbias + curr_imu_unbias);

    double dtheta = imu_unbias_hat(2) * dt;

    // Eigen::Matrix<double,2,2> R = math_utils::rotation(dtheta);
    Eigen::Matrix<double,2,2> R = math_utils::rotation(last_imu_state.theta);

    // update q, also plus dtheta, reason of this data
    imu_state.theta += dtheta;
    imu_state.theta = atan2(sin(last_imu_state.theta), cos(last_imu_state.theta));

    
    // std::cout << "dtheta ============= : " << dt << ", " << dtheta << std::endl;
    // std::cout << "state.theta ============= : " << dt << ", " << state.theta << std::endl;

    // update p 
    Eigen::Vector2d acc_hat(imu_unbias_hat[0], imu_unbias_hat[1]);
    imu_state.p += last_imu_state.v * dt + 0.5 * R.transpose() * acc_hat * dt * dt;

    // update v 
    imu_state.v += R.transpose() * acc_hat * dt;
}